#include <iostream>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/visualization/cloud_viewer.h>
#include <string>
#include <unistd.h>
#include <thread>

using namespace std;

string frame_id;
string topic_id;
string node_name;

pcl::visualization::CloudViewer viewer("Viewer");

bool msg_reced = false;


void viewerPsycho (pcl::visualization::PCLVisualizer& viewer)
{
    unsigned count = 0;
    std::cout<< "Once per viewer loop: " << count<<std::endl;
}

int getParam(ros::NodeHandle nh)
{
    node_name = ros::this_node::getName();

    ROS_INFO("node_name: %s", node_name.c_str());

    nh.param<std::string>(node_name+"/topic_id", topic_id,"/points");
    nh.param<std::string>(node_name+"/frame_id", frame_id,"/platform");
    return 0;
}

void show_pointcloud(sensor_msgs::PointCloud2 msg){

    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::fromROSMsg<pcl::PointXYZRGB>(msg,*cloud);
    ROS_INFO(msg.header.frame_id.c_str());
	viewer.showCloud(cloud);
    msg_reced = true;
}

int main(int argc,char** argv)
{
    ros::init (argc, argv, "empty");

    ros::NodeHandle nh;

    ROS_INFO("Current working directory: %s",get_current_dir_name());

    getParam(nh);

    ros::Subscriber pcl_sub = nh.subscribe<sensor_msgs::PointCloud2> (topic_id, 1, show_pointcloud);

    while(!msg_reced){
        ros::spinOnce();
    }

    viewer.runOnVisualizationThread(viewerPsycho);

 	while (!viewer.wasStopped())
	{

	}

    return 0;
}
